/*************************************************************************
*
*	engineering_main.c
*
*  Copyright 2007,2008,2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for interpreting command files
*    and following a waypoint path, and calls routines in
*    simulate_boat in order to update boat telemetry.
*
*
**************************************************************************
*
*  version 2.0.4, 2/27/09, SBS
*    - remove randomized initial heading
*
*  version 2.0.3, 2/23/09, SBS
*    - add platformtype in STATUS
*    - cleanup
*
*  version 2.0.2, 2/2/09, SBS
*    - add ruddercommand and throttlecommand output to telemfile
*
*  version 2.0.1, 2/1/09, SBS
*    - reconcile with new COMMAND struct
*    - reconcile with new STATUS struct
*
*  version 2.0, 1/9/09, SBS
*    - rearrange code so that it pulls current STATUS and COMMAND from
*       platformstatus() functions
*    - eliminate all simulation calls for now
*    - eliminate interrupr/resume for now
*    - this module no longer has any memory
*
*  version 1.3.9, 1/8/09, SBS
*    - LogPrint --> LogLevelPrint
*
*  version 1.3.8, 12/31/08, SBS
*   - get boatname from config file
*
*  version 1.3.7, 11/28/08, SBS
*   - modify calls into commandfile_ and telemfile_ to pass filespec
*
*  version 1.3.6, 11/26/08, SBS
*   - remove telemetry file writing
*
*  version 1.3.5, 11/19/08, SBS
*   - incorporate compass
*
*  version 1.3.4, 10/23/08, SBS
*   - incorporate gpsd
*
*  version 1.3.3, 10/17/08, SBS
*   - reorganize to handle real platforms
*
*  version 1.3.2, 10/4/08, SBS
*   - documentation
*
*  version 1.3.1, 8/27/08, SBS
*   - use status_SetTimeString()
*   - cleanup
*
*  version 1.3, 8/18/08, SBS
*    - change to having waypoint list stored in STATUS instead of referring back to COMMAND   
*
*  version 1.2.10, 8/15/08, SBS
*    - fix bugs in interrupt/uninterrupt (used wrong array index)
*    - cleanup
*
*  version 1.2.9, 7/5/08, SBS
*    - add INTERRUPT/UNINTERRUPT
*
*  version 1.2.8, 5/30/08, SBS
*    - add initialization of command structs in Initialize()
*    - move conversions (lat, long, deg, rad, etc) into conversions.h
*    - add pause/resume functionality
*    - cleanup
*
*  version 1.2.7, 5/29/08, SBS
*    - move speed control into simulate_boat
*    - move InitBoat into simulate_boat
*
*  version 1.2.6, 5/18/08, SBS
*    - add random initial heading
*
*  version 1.2.5, 5/17/08, SBS
*    - add turning rate to heading changes
*
*  version 1.2.4, 5/5/08, SBS
*    - rename groundspeed --> waterspeed
*
*  version 1.2.3, 4/30/08, SBS
*    - add groundspeed
*
*  version 1.2.2, 4/3/08, SBS
*    - fix problems when DEBUG is not defined
*
*  version 1.2.1, 3/5/08, SBS
*    - add #ifdef MAPDEMO with special starting locations for boats
*    - minor cleanup
*
*  version 1.2, 2/25/08, SBS
*    - move retrieval of command file into simulator.c
*    - move headingfromwaypoint, distancefromwaypoint to waypoints.c
*
*  version 1.1, 1/13/08, SBS
*    - multiple waypoints
*    - multiple robots
*    - split boat prediction routines out into simulate_boat.c
*    - etc.
*
*  version 1.0, 12/17/07, SBS
*    - split from simulator.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @defgroup plat_engineering
*	@ingroup platform
*  This module simulates properties of the boat itself.
*/
/**
*  @file 
*  @ingroup plat_engineering
*  @brief
*  This module provides functions for interpreting command files
*    and following a waypoint path, and calls routines in
*    simulate_boat in order to update boat telemetry.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <math.h>

#include <stdint.h>
#include "types.h"

#include "ssa.h"

#include "waypoints.h"
#define NEED_TYPENAMES
#include "platform_types.h"
#undef NEED_TYPENAMES
#define NEED_MODENAMES
#include "platform_modes.h"
#undef NEED_MODENAMES
#include "status.h"

#include "config.h"

#include "conversions.h"

#include "command.h"

#ifdef DEBUG
#include "logfile.h"
#endif

///#ifdef ENGINEERING_SIMULATE_POSITION
///#include "simulate_position.h"
///#endif

///#ifdef ENGINEERING_SIMULATE_SPEED
///#include "simulate_speed.h"
///#endif

///#ifdef ENGINEERING_SIMULATE_HEADING
///#include "simulate_heading.h"
///#endif

#ifdef ENGINEERING_SIMULATE_BATTERY
#include "simulate_battery.h"
#endif

///#ifdef ENGINEERING_SIMULATE_ATTITUDE
///#include "simulate_attitude.h"
///#endif

#ifdef ENGINEERING_USE_GPSD
#include "gpsd.h"
#endif

#ifdef ENGINEERING_USE_COMPASS
#include "compass.h"
#endif

#ifdef ENGINEERING_USE_SONDE
#include "sonde.h"
#endif

#include "controller.h"
#include "platformstatus.h"
#include "engineering_main.h"

//#include "../simulator.h" 

#define MAXBOATS 1


/********************************************************/
////need to get these from config file
#define CLOSEENOUGH	(10)
#define BOATINITLON	(-79.9)
#define BOATINITLAT	(40.6)
#define COMPASSPORT	"/dev/compass0"
#define GPSDHOSTIP	"127.0.0.1"
#define GPSDHOSTPORT	"2947"
#define SONDEPORT "/dev/ttyS0"
/********************************************************/



/**
***************************************************************************
*
* UpdateEngStatus()
*/
/**
* @brief
* Update engineering status for a single boat.
*
*
********************************************************/
static void UpdateEngStatus(STATUS *s)
{

//	#ifdef ENGINEERING_SIMULATE_POSITION
//	UpdateBoatPosition_sim(s,deltat);
//	#endif

//	#ifdef ENGINEERING_SIMULATE_SPEED
//	UpdateBoatSpeed_sim(s,deltat);
//	#endif
	
	#ifdef ENGINEERING_USE_GPSD
	gpsd_GetStatus(s);
	#endif

	#ifdef ENGINEERING_USE_COMPASS
	compass_GetStatus(s);
	#endif
	
	#ifdef ENGINEERING_USE_SD84
	controller_GetStatus(s);
	//	sd84_GetStatus();
	#endif
	
//	#ifdef ENGINEERING_SIMULATE_HEADING
//	UpdateBoatHeading_sim(s,deltat,targetHdg);
//	#endif
	
//	#ifdef ENGINEERING_SIMULATE_ATTITUDE
//	UpdateBoatOrientation_sim(s,deltat);
//	#endif
	
	#ifdef ENGINEERING_SIMULATE_BATTERY
	SimBattery_GetStatus(s);
	#endif
	
	#ifdef ENGINEERING_USE_SONDE
	sonde_GetStatus(s);
	#endif
	
	return;
}


/**
***************************************************************************
*
* engineering_InitStatus()
*/
/**
* @brief
* Set initial values for engineering status
*
********************************************************/
STATUS engineering_InitStatus(
			int boatnum
			,char *boatname
			,int plattype)
{
	STATUS s;
	time_t t;
	struct tm *tmp;
	
	printf("engineering_InitStatus()\n");

	t = time(NULL);
	tmp = localtime(&t);		
	
	s = status_InitStatus();

	//**********
	// fill general info into status struct
	//**********

	s.nPlatformID = boatnum + 1;	
	
	snprintf(s.sPlatformName, sizeof(s.sPlatformName)
			, "%s", boatname);

	s.nPlatformType = plattype;	
	snprintf(s.sPlatformTypeName, sizeof(s.sPlatformTypeName)
			, "%s",  platformtypenames[s.nPlatformType]);
	

	s.nPlatformMode = MODE_AWAITINSTR;
	snprintf(s.sPlatformModeName, sizeof(s.sPlatformModeName)
			, "%s",  modenames[s.nPlatformMode]);
				
	s.nWaypointCount = 0;

	status_SetTimeString(s.sTime, sizeof(s.sTime),tmp);

//// read initial position from config file
	s.dLonDeg = BOATINITLON;
	s.dLatDeg = BOATINITLAT;
	

//	s.dHdgDeg = rand() / (((double)RAND_MAX + 1.) / 360.);           
	
	s.dSpeed_mps = 0.;       
	s.dWaterspeed_mps = 0.;       
	s.dDirectionDeg = 0.;       

	#ifdef DEBUG2
	status_PrintStatus(s);
	#endif
	
	#ifdef ENGINEERING_USE_SONDE
	sonde_Start(SONDEPORT);	
	#endif 

	#ifdef ENGINEERING_USE_COMPASS
	compass_Start(COMPASSPORT);
	#endif
	
	#ifdef ENGINEERING_USE_GPSD
	gpsd_Init(GPSDHOSTIP,GPSDHOSTPORT);
//	gpsd_StartThread();
	#endif
	
	#ifdef ENGINEERING_SIMULATE_BATTERY
	SimBattery_Init(&s);
	#endif

	#ifdef ENGINEERING_USE_SD84
	controller_InitStatus(&s);
	//	sd84_InitStatus();
	#endif
	
	
	return(s);
}


/**
***************************************************************************
*
* ProcessCommand()
*/
/**
* @brief
* Changes boat modes according to the data in a command 
*    file
*
********************************************************/
/*
static void ProcessCommand(
			 CONFIGDATA cf
			,COMMAND cNew
			,int boatnum
			,COMMAND *cOld
			,STATUS *s)
{
	
	static COMMAND c_interrupt[MAXBOATS];	// memory of what the command was before an interrupt
	static STATUS  s_interrupt[MAXBOATS];	// memory of what the status was before an interrupt
	static int interrupted[MAXBOATS];
	static int memory=0;

	int index = boatnum-1;

	#ifdef DEBUG
	static char temp[LOGSTRLEN];
	#endif
	
	#ifdef DEBUG2
	snprintf(temp,LOGSTRLEN,"Hello from ProcessCommand() - boat #%d",boatnum);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	
	command_PrintCommand(*cOld);
	command_PrintCommand(cNew);
	#endif

	if(memory==0){
		register int i;
		memory=1;
		for(i=0;i<MAXBOATS;i++){
			interrupted[i]=0;
		}
	}
		

	// **********
	// check if command file changed
	// **********
	if(command_CompareCommand(*cOld,cNew) == 1){
	
		#ifdef DEBUG	
		snprintf(temp,LOGSTRLEN,"ProcessCommand() - Command changed");
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		#endif
		
		// **********
		// set new boat mode
		// **********
		switch(cNew.nCmdID){
		
			case CMD_NULL:		// continue in current mode

				#ifdef DEBUG2
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command NULL");
				#endif

				*cOld = cNew;

				break;
	
			case CMD_HALT:		// stop boat, discard waypoints

				#ifdef DEBUG2
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command HALT");
				#endif
				
				s->nPlatformMode = MODE_AWAITINSTR;
				
				s->nWaypointCount = 0;

				*cOld = cNew;

				break;
				
			case CMD_WPT:		// follow waypoints	

				#ifdef DEBUG2
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command WPT");
				#endif
				
				s->nPlatformMode = MODE_TRANSIT;
			
				s->nCurrWaypt = 0;
				s->nWaypointCount = cNew.nWaypointCount;
				s->CurrWpt = cNew.Waypoints[s->nCurrWaypt];

				{
					register int i;							
					for(i=0;i<s->nWaypointCount;i++){
						s->Waypoints[i] = cNew.Waypoints[i];				
					}
				}
				
				*cOld = cNew;
				
				break;
		
			case CMD_PAUSE:		// stop boat, keep waypoints
				#ifdef DEBUG2
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command PAUSE");
				#endif
				
				s->nPlatformMode = MODE_PAUSE;
				
				*cOld = cNew;

				break;
		
			case CMD_RESUME:		// resume existing waypoints

				#ifdef DEBUG2
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command RESUME");
				#endif

				if(s->nWaypointCount >0){
					s->nPlatformMode = MODE_TRANSIT;
				}else{
					s->nPlatformMode = MODE_AWAITINSTR;
				}

				*cOld = cNew;

				break;		
				
			case CMD_INTERRUPT:	// save current command in order to execute a new command

				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command INTERRUPT");
				#endif

				if(interrupted[index] == 1){
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_INFO,"This boat is already interrupted, not saving state");
					#endif
				}else	{				

					c_interrupt[index] = *cOld;
					s_interrupt[index] = *s;
					interrupted[index] = 1;

					{
						register int i;							
						for(i=0;i<s->nWaypointCount;i++){
							s_interrupt[index].Waypoints[i] = s->Waypoints[i];				
						}
					}
				
					#ifdef DEBUG
					snprintf(temp,LOGSTRLEN
								,"===== INTERRUPT ===== current waypoint = %d\n"
								,s_interrupt[index].nCurrWaypt);
					logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
					snprintf(temp,LOGSTRLEN
								,"===== INTERRUPT ===== current waypoint = %d\n"
								,s_interrupt[index].nCurrWaypt);
					logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
					waypoints_WaypointPrintWithString(
								"===== INTERRUPT ===== current waypoint: "
								,s->Waypoints[s->nCurrWaypt]);
					waypoints_WaypointPrintWithString(
								"===== INTERRUPT ===== current waypoint: "
								,s_interrupt[index].Waypoints[s_interrupt[index].nCurrWaypt]);
					waypoints_WaypointPrintWithString(
								"===== INTERRUPT ===== current waypoint: "
								,s_interrupt[index].CurrWpt);
					#endif
				}
				
				s->nPlatformMode = MODE_INTERRUPT;

				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - ******* interrupt mode");
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - current status");
				status_PrintStatus(*s);
				
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - old command");
				command_PrintCommand(*cOld);

				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - current command");
				command_PrintCommand(cNew);

				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - ******* interrupt mode");
				#endif

				*cOld = cNew;

				break;				
			
			case CMD_UNINTERRUPT:
			
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - Processing command UNINTERRUPT");
				#endif

				if(interrupted[index] == 0){
					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_INFO,"This boat is not interrupted");
					#endif
//					*cOld = cNew;
				}else	{				

					#ifdef DEBUG
					logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - ******* uninterrupt mode\n");
					logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - interrupt status");
					status_PrintStatus(s_interrupt[index]);
					logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - interrupt command");
					command_PrintCommand(c_interrupt[index]);
					logfile_LogLevelPrint(LOGLEVEL_INFO,"ProcessCommand() - ******* uninterrupt mode\n");
					#endif
			
					s->nPlatformMode = s_interrupt[index].nPlatformMode;
					strncpy(s->sPlatformModeName,s_interrupt[index].sPlatformModeName,MODENAMESTRLEN);
					s->nWaypointCount = s_interrupt[index].nWaypointCount;
					s->nCurrWaypt = s_interrupt[index].nCurrWaypt;
					
					s->CurrWpt = s_interrupt[index].CurrWpt;
					
					{
						register int i;
						for(i=0;i<s->nWaypointCount;i++){
							s->Waypoints[i] = s_interrupt[index].Waypoints[i];
						}
					}				

					#ifdef DEBUG
					snprintf(temp,LOGSTRLEN,"===== UNINTERRUPT ===== current waypoint = %d\n",s->nCurrWaypt);
					logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
					waypoints_WaypointPrintWithString(
								"===== UNINTERRUPT ===== current waypoint: "
								,s->Waypoints[s->nCurrWaypt]);
					waypoints_WaypointPrintWithString(
								"===== UNINTERRUPT ===== current waypoint: "
								,s->CurrWpt);
					#endif

//					*cOld = c_interrupt[index];
//(					ProcessCommand(cf
//						, c_interrupt[index]
//						, boatnum
//						, cOld
//						, s
//						);

				}
				
				*cOld = cNew;

				break;
				
			default:

				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_INFO,"Processing command UNKNOWN");
				#endif

				printf("Processing command UNKNOWN\n");

				*cOld = cNew;

				break;
				
		}
	
//				printf("hello\n");
		strncpy(s->sPlatformModeName, modenames[s->nPlatformMode], sizeof(s->sPlatformModeName));
//				printf("hello\n");

	}

	return;
}
*/


/**
***************************************************************************
*
* DealWithWaypoints()
*/
/**
* @brief
* When the boat is within a prescribed radius of the
*    current target waypoint, this function increments
*    the waypoint, or if there are no more waypoints, 
*    changes the boat mode to "Awaiting Instructions"
*   
*
********************************************************/
static void DealWithWaypoints(
			STATUS *s)
{
	double dist;
	WAYPT p;

	#ifdef DEBUG
	char temp[LOGSTRLEN];
	snprintf(temp, LOGSTRLEN, "DealWithWaypoints() - "
				"Waypoint goal is (%10.7lf,%10.7lf) @ %6.3lf m/s"
				, s->CurrWpt.dLatDeg
				, s->CurrWpt.dLonDeg
				, s->CurrWpt.dSpeedMps);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif		

	//**********
	// check if waypoint reached
	//**********
	
	p = status_WaypointFromStatus(*s) ;
	dist = waypoints_DistanceFromWaypoint(p, s->CurrWpt) ;

	#ifdef DEBUG	
	snprintf(temp, LOGSTRLEN, 
					"DealWithWaypoints() - "
					"Distance to waypoint is %lf"
					,dist);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif
	
	if(dist < CLOSEENOUGH){
		
		#ifdef DEBUG	
		logfile_LogLevelPrint(LOGLEVEL_INFO
				,"DealWithWaypoints() - Waypoint achieved");
		snprintf(temp,LOGSTRLEN,"current waypoint is %d\n"
				,s->nCurrWaypt);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		snprintf(temp,LOGSTRLEN,"waypoint count is %d\n"
				,s->nWaypointCount);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		#endif		
		
		(s->nCurrWaypt)++;
		
		if((s->nCurrWaypt) < (s->nWaypointCount)){

			// next waypoint

			#ifdef DEBUG	
			snprintf(temp,LOGSTRLEN,"advancing to waypoint #%d\n"
					,s->nCurrWaypt);
			logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
			#endif

			s->CurrWpt = s->Waypoints[s->nCurrWaypt];
			
		}else{

			// finished path

			#ifdef DEBUG	
			snprintf(temp,LOGSTRLEN,"reached last waypoint\n");
			logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
			#endif

			s->nPlatformMode = MODE_AWAITINSTR;

			strncpy(s->sPlatformModeName,modenames[s->nPlatformMode]
					,sizeof(s->sPlatformModeName));
		}
	}
	
	return;
}


/********************************************************
*
* engineering_update()
*/
/**
* @brief
* This is the entry point for boat simulation.
*    It is called from the platform() module.
*
*
********************************************************/
/*
STATUS engineering_update_old(
			CONFIGDATA cf
			, COMMAND c_new
			,STATUS oldstatus
			, int boatnum)
{
	static time_t oldtime;
	static STATUS s;
	static COMMAND c_old;
	static int memory=0;

	double TargetHeading;
	int deltat;
//	int index = boatnum-1;
	
	
	#ifdef DEBUG
	static char temp[LOGSTRLEN];
	
	snprintf(temp,LOGSTRLEN,"engineering_main() - robot #%d", boatnum);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif	
	
	// **********
	// initialize data on first run
	// **********
	
	if(memory==0){
		time_t t;
		struct tm *tmp;
	
		t = time(NULL);
		tmp = localtime(&t);		
	
		memory=1;
//		s = engineering_Init(c_old,oldtime,boatnum-1);
	
		printf("******************\n");
		s = InitBoat(boatnum,cf.robotname,tmp);		
		c_old = command_InitCommand();
		oldtime = t;

		#ifdef DEBUG2
		status_PrintStatus(s);
		#endif
		
		return(s);
	}else{
	
		s = oldstatus;
	}
	
	#ifdef DEBUG2
	status_PrintStatus(oldstatus);
	#endif
	
//	printf("hello2\n");
	
	// **********
	// set time in status struct and calculate elapsed time
	// **********
	
	deltat = settime(&s,&oldtime);
		
		
////	if(index < SIMBUOYOFFSET){ // don't bother with command files and waypoints if it's a buoy
//// need to check "type" instead

		// **********
		// parse command file
		// **********
		
		ProcessCommand(
				cf
				,c_new
				,boatnum
				,&c_old
				,&s
				);
	
		#ifdef DEBUG
		
		snprintf(temp, LOGSTRLEN, "engineering_main() - "
					"Current Position is (%10.7lf,%10.7lf)"
					, s.dLatDeg
					, s.dLonDeg);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);

		snprintf(temp, LOGSTRLEN, "engineering_main() - "
					"Current Attitude is (%5.2lf,%5.2lf,%5.2lf)"
					, s.dHdgDeg
					, s.dPitchDeg
					, s.dRollDeg);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	

		snprintf(temp, LOGSTRLEN
					,"engineering_main() - current command is %s"
					, c_old.sCmdName);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		
		snprintf(temp, LOGSTRLEN
					,"engineering_main() - current mode is %s"
					, modenames[s.nPlatformMode]);
		logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
		
		#endif		
	

		// **********
		// monitor waypoint achievement
		// **********
		
		if(s.nPlatformMode == MODE_TRANSIT){
			
//			DealWithWaypoints((c_old[index]),&(s[index]));
			DealWithWaypoints(&s);
			
		}
			
		
		// **********
		// determine goal heading
		// **********
			
		if(s.nPlatformMode == MODE_TRANSIT){
	
			WAYPT w;
			w = status_WaypointFromStatus(s);
			TargetHeading = waypoints_HeadingFromWaypoint(w, s.CurrWpt);
			
		}else{
		
			TargetHeading = 512.0;	// large number means it's adrift
		}
		
//	}//if(index < SIMBUOYOFFSET)
	
	
	// **********
	// update boat position, heading, etc.	
	// **********

	UpdateBoat(&(s),deltat,TargetHeading);
	
	
	#ifdef DEBUG
	snprintf(temp, LOGSTRLEN, "engineering_main() - "
					"Updated Position is (%10.7lf,%10.7lf)"
					, s.dLatDeg
					, s.dLonDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);

	snprintf(temp, LOGSTRLEN, "engineering_main() - "
				"Updated Attitude is (%5.2lf,%5.2lf,%5.2lf)"
				, s.dHdgDeg
				, s.dPitchDeg
				, s.dRollDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,temp);
	#endif

	
	return(s);
}
*/


/**
***************************************************************************
*
* HandleCommand()
*/
/**
* @brief
* Changes boat modes according to the current COMMAND struct
*
**************************************************************************/

static void HandleCommand(
//			 CONFIGDATA cf
			COMMAND cmd
			,STATUS *st)
{
	#ifdef DEBUG
//	static char temp[LOGSTRLEN];

	logfile_LogLevelPrint(LOGLEVEL_DEBUG,"Hello from ProcessCommand()");
	command_PrintCommand(cmd);
	#endif


	//**********
	// set new boat mode
	//**********

	switch(cmd.nCmdID){
		
		case CMD_NULL:		// continue in current mode

			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command NULL");
			#endif

			break;
	
		case CMD_HALT:		// stop boat, discard waypoints
								// currently sets to free drift
								// eventually we should have separate commands for free drift and station keep
								// but then we need to figure out how to deal with OASIS...

			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command HALT");
			#endif
				
//			st->nPlatformMode = MODE_AWAITINSTR;
			st->nPlatformMode = MODE_FREEDRIFT;

			st->nWaypointCount = 0;

			controller_SetModeFreeDrift();
			
			break;
				
		case CMD_WPT:		// follow waypoints	

			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command WPT");
			#endif
				
//			st->nPlatformMode = MODE_TRANSIT;
			st->nPlatformMode = MODE_WAYPOINT;
			
			st->nCurrWaypt = 0;
			st->nWaypointCount = cmd.nWaypointCount;
			st->CurrWpt = cmd.Waypoints[st->nCurrWaypt];

			{
				register int i;							
				for(i=0; i<st->nWaypointCount; i++){
					st->Waypoints[i] = cmd.Waypoints[i];				
				}
			}
					
			if(st->nWaypointCount > 1){
				controller_SetModeWaypoint(st->Waypoints[0],st->Waypoints[1]);
			}else{
				controller_SetModeWaypoint(st->Waypoints[0],st->Waypoints[0]);
			}

			break;
		
		case CMD_PAUSE:		// stop boat, keep waypoints
				#ifdef DEBUG
				logfile_LogLevelPrint(LOGLEVEL_INFO
						,"HandleCommand() - Processing command PAUSE");
				#endif
				
				st->nPlatformMode = MODE_PAUSE;
				
				controller_SetModeFreeDrift();

				break;
		
		case CMD_RESUME:		// resume existing waypoints

			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command RESUME");
			#endif

			if(st->nWaypointCount >0){
				st->nPlatformMode = MODE_TRANSIT;
	
				if(cmd.nWaypointCount > 1){
					controller_SetModeWaypoint(st->Waypoints[0],st->Waypoints[1]);
				}else{
					controller_SetModeWaypoint(st->Waypoints[0],st->Waypoints[0]);
				}
			}else{
				st->nPlatformMode = MODE_AWAITINSTR;
			}

			break;		
				
		case CMD_INTERRUPT:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command INTERRUPT");
			#endif

			break;

		case CMD_UNINTERRUPT:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command UNINTERRUPT");
			#endif
			break;
			
		case CMD_MANUAL:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command MANUAL");
			#endif

			st->nPlatformMode = MODE_MANUAL;
			st->nWaypointCount = 0;
			controller_SetModeManual(cmd.nRudder,cmd.nThrottle);
			break;

		case CMD_CALIBRATE:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command CALIBRATE");
			#endif

			st->nPlatformMode = MODE_CALIBRATE;
			st->nWaypointCount = 0;
			controller_SetModeCalibrate();

			break;

		case CMD_STATIONKEEP:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command STATIONKEEP");
			#endif

			st->nPlatformMode = MODE_STATIONKEEP;
			st->nCurrWaypt = 0;
			st->nWaypointCount = 1;
			st->CurrWpt = cmd.Waypoints[0];
			st->Waypoints[0] = cmd.Waypoints[0];				
					
			controller_SetModeStationKeep(st->Waypoints[0]);

			break;

		case CMD_FREEDRIFT:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command FREEDRIFT");
			#endif

			st->nPlatformMode = MODE_FREEDRIFT;
			st->nWaypointCount = 0;
			controller_SetModeFreeDrift();

			break;

		case CMD_HEADINGHOLD:
			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command HEADINGHOLD");
			#endif

			st->nPlatformMode = MODE_HEADINGHOLD;
			st->nWaypointCount = 0;
			controller_SetModeHeadingHold(cmd.nHeading,cmd.nThrottle);

			break;
		
		default:

			#ifdef DEBUG
			logfile_LogLevelPrint(LOGLEVEL_INFO
					,"HandleCommand() - Processing command UNKNOWN");
			#endif

			printf("HandleCommand() - Processing command UNKNOWN\n");

			break;

	}
	strncpy(st->sPlatformModeName, modenames[st->nPlatformMode]
			, sizeof(st->sPlatformModeName));

	return;
}


/**
***************************************************************************
*
* engineering_update()
*/
/**
* @brief
* This is the entry point for boat simulation.
*    It is called from the platform() module.
*
*
**************************************************************************/
STATUS engineering_update(void)
{
	STATUS st;

	time_t t;
	struct tm *tmp;
	
	
	#ifdef DEBUG
	static char logtemp[LOGSTRLEN];
	#endif
		

	st = platformstatus_GetStatus();

	
	t = time(NULL);
	tmp = localtime(&t);		
	status_SetTimeString(st.sTime, sizeof(st.sTime),tmp);
		
		
	
	#ifdef DEBUG
		
	snprintf(logtemp, sizeof(logtemp), "engineering_main() - "
				"Current Position is (%10.7lf,%10.7lf)"
				, st.dLatDeg
				, st.dLonDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);

	snprintf(logtemp, sizeof(logtemp), "engineering_main() - "
				"Current Attitude is (%5.2lf,%5.2lf,%5.2lf)"
				, st.dHdgDeg
				, st.dPitchDeg
				, st.dRollDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);



	snprintf(logtemp, sizeof(logtemp)
				,"engineering_main() - current mode is %s"
				, modenames[st.nPlatformMode]);
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);
		
	#endif		
	


	UpdateEngStatus(&st);

	if(platformstatus_CommandChanged() == 1){
		COMMAND cmd;

		cmd = platformstatus_GetCommand();

		#ifdef DEBUG
		snprintf(logtemp, sizeof(logtemp)
				,"engineering_main() - current command is %s"
				, cmd.sCmdName);
		logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);
		#endif		
		
		HandleCommand(cmd,&st);
	}
		
	
	#ifdef DEBUG
	snprintf(logtemp, sizeof(logtemp), "engineering_main() - "
				"Updated Position is (%10.7lf,%10.7lf)"
				, st.dLatDeg
				, st.dLonDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);

	snprintf(logtemp, sizeof(logtemp), "engineering_main() - "
				"Updated Attitude is (%5.2lf,%5.2lf,%5.2lf)"
				, st.dHdgDeg
				, st.dPitchDeg
				, st.dRollDeg);
	logfile_LogLevelPrint(LOGLEVEL_INFO,logtemp);
	#endif

	
	return(st);
}

